#pragma config(Hubs,  S1, HTMotor,  HTServo,  HTMotor,  none)
#pragma config(Sensor, S1,     ,               sensorI2CMuxController)
#pragma config(Motor,  mtr_S1_C1_1,     motorBL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C1_2,     motorFL,       tmotorTetrix, openLoop, encoder)
#pragma config(Motor,  mtr_S1_C3_1,     motorFR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Motor,  mtr_S1_C3_2,     motorBR,       tmotorTetrix, openLoop, reversed, encoder)
#pragma config(Servo,  srvo_S1_C2_1,    servoFR,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_2,    servoBR,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_3,    servoFL,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_4,    servoBL,              tServoStandard)
#pragma config(Servo,  srvo_S1_C2_5,    wrist,                tServoStandard)
#pragma config(Servo,  srvo_S1_C2_6,    servo6,               tServoNone)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#include "JoystickDriver.c";


float slowMult = 1.0;
int deadZone = 10;

/*void ArmControlFunc()
{

	if (abs(joystick.joy2_x1) > deadZone)
	{
		motor [lazySuzan] = joystick.joy1_x1;
	}
	if (abs(joystick.joy2_y1) > deadZone)
	{
		motor [arm] = joystick.joy2_y1;
	}
	if (joy2Btn(4) == 1)//left bumper
	{
		servo [wrist] = 100;//this value will change, i don't know the servo value system well, or the orientation of the servo
		//this also goes with all the other servo values

	}
	if (joy2Btn(6) == 1)//left trigger
	{
		servo [wrist] = 200;
	}
	if (joy2Btn(7) == 1)//right trigger
	{
		//servo [brush] ++ 360;//i don't know how to use continual rotation servos
	}
}

	task armControl()
	{
		while(true)
		{
			getJoystickSettings(joystick);
			if (abs(joystick.joy2_y1) > deadZone)
			{
				ArmControlFunc();
			}
		}
	}
*/
	task main()
	{
		servo[servoFL] = 0;
		servo[servoFR] = 0;
		servo[servoBL] = 0;
		servo[servoBR] = 0;

		waitForStart();

		while (true)
		{
			getJoystickSettings(joystick);
			//StartTask(armControl);

			int pos = atan(joystick.joy1_y2 / joystick.joy1_x2);
			float dist = sqrt(joystick.joy1_y2 * joystick.joy1_y2 + joystick.joy1_x2 * joystick.joy1_x2);

			if (joystick.joy1_Buttons == 6)
			{
				slowMult = 1;
			}
			else
			{
				slowMult = 0.3;
			}

			float motorPower = dist * slowMult;


			if (dist > deadZone)
			{
				motor[motorFL] = motorPower;
				motor[motorFR] = motorPower;
				motor[motorBL] = motorPower;
				motor[motorBR] = motorPower;

				float servoPos = pos * 1.416;

				servo[servoFL] = servoPos;
				servo[servoFR] = servoPos;
				servo[servoBL] = servoPos;
				servo[servoBR] = servoPos;
			}
			else if (dist < (-1 * deadZone))
			{
				motor[motorFL] = motorPower;
				motor[motorFR] = motorPower;
				motor[motorBL] = motorPower;
				motor[motorBR] = motorPower;

				float servoPos = (pos + 180) * 1.416;

				servo[servoFL] = servoPos;
				servo[servoFR] = servoPos;
				servo[servoBL] = servoPos;
				servo[servoBR] = servoPos;
			}
			else
			{
				motor[motorFL] = 0;
				motor[motorFR] = 0;
				motor[motorBL] = 0;
				motor[motorBR] = 0;
			}


		}

	}
